#ifndef PCL_NO_PRECOMPILE
#define PCL_NO_PRECOMPILE
#endif

#include <memory>
#include <chrono>
#include <string>
#include <boost/make_shared.hpp>
#include <vector>
#include <cmath>
#include "rclcpp/rclcpp.hpp"
//#include "std_msgs/msg/string.hpp"
#include <sensor_msgs/msg/laser_scan.hpp>
#include "sensor_msgs/msg/point_cloud2.hpp"

#include "sensor_msgs/msg/imu.hpp"
#include "std_msgs/msg/header.hpp"

#include <pcl/point_cloud.h>
#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>
//#include <pcl_ros/point_cloud.h>
#include <pcl_conversions/pcl_conversions.h>
#include <pcl/common/common.h>
#include <pcl/common/transforms.h>
#include <pcl/filters/voxel_grid.h>
//#include <pcl/octree/octree.h>
#include <pcl/kdtree/kdtree_flann.h>
//#include <pcl/registration/icp.h>
//#include <pcl/features/normal_3d.h>

// Eigen
#include <Eigen/Core>
#include <Eigen/Eigen>
#include <Eigen/Geometry>
#include "scan_process.hpp"
#include "file_sl.hpp"
#include "calculete_seat.hpp"
#include <fstream>
using namespace std;
#define SCAN_CLOUD 1
class BlueseaSubScan: public rclcpp::Node
{
    public:
        BlueseaSubScan(): Node("process_bluesea")
        {
            using std::placeholders::_1;
            scan_p=new Scan_cloud();
            if(SCAN_CLOUD==0)
            sub_bluesea_cloud = this->create_subscription<sensor_msgs::msg::PointCloud2>(
                "/cloud", 
                10, std::bind(&BlueseaSubScan::topic_callback_cloud, this, std::placeholders::_1));
            if(SCAN_CLOUD==1)
            sub_bluesea_scan = this->create_subscription<sensor_msgs::msg::LaserScan>(
                "/scan", 
                10, std::bind(&BlueseaSubScan::topic_callback_scan, this, std::placeholders::_1));
            publisher_ = this->create_publisher<sensor_msgs::msg::PointCloud2>("/my_cloud", 10);
            angle_pub_ = this->create_publisher<sensor_msgs::msg::PointCloud2>("/my_angle", 10);
            ag_cloud=new Angle_Point_Cloud();
           // scloud->read_cloud();
        }
    private:
     void topic_callback_cloud(const sensor_msgs::msg::PointCloud2::SharedPtr msg) 
     {
         size_t size;
         double x,y,max_x,max_y;
         pcl::PointCloud<pcl::PointXYZ> lidar_cloud;
        
         sensor_msgs::msg::PointCloud2 mymsg;
         pcl::fromROSMsg(*msg, lidar_cloud);
            size =lidar_cloud.points.size();
       // for(int32_t i=0;i<20;i++)    
       //   RCLCPP_INFO(this->get_logger(),"i=%d  x=%5.2f y=%5.2f,z=%5.2f",i,lidar_cloud.points[i].x,lidar_cloud.points[i].y,lidar_cloud.points[i].z);
        pcl::PointCloud<pcl::PointXYZ>::Ptr trybb_cloud(new pcl::PointCloud<pcl::PointXYZ>);
        for(auto &point:lidar_cloud.points)
         {
            pcl::PointXYZ &p = point;
            if(p.x>0)
            {
                p.z=0.3;
                trybb_cloud->push_back(p);
            }
         }
            pcl::toROSMsg(*trybb_cloud, mymsg);
            mymsg.header.stamp = now();
            mymsg.header.frame_id="map";
            publisher_->publish(mymsg);
     }
    void topic_callback_scan(const sensor_msgs::msg::LaserScan::SharedPtr scan) 
     {
      
       int num_points = scan->ranges.size();
         sensor_msgs::msg::PointCloud2 mymsg;
         sensor_msgs::msg::PointCloud2 angle_msg;
         //std::vector<int>* angle_index_;
         int value;
       // 初始化点云
        pcl::PointCloud<pcl::PointXYZ> angle_cloud;
        pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_ (new pcl::PointCloud<pcl::PointXYZ>);
        pcl::PointXYZ point_temp;
      //  cloud_=std::make_shared<pcl::PointCloud<pcl::PointXYZ>>(); // 使用 make_shared 创建并赋值;
       // *cloud_=lidar_cloud;
      // lidar_cloud.resize(num_points);
        scan_p->set_scan_input(scan);
        scan_p->convert_to_cloud(cloud_);
        scan_p->find_angle_point();
       pcl::toROSMsg(* cloud_, mymsg);
            mymsg.header.stamp =now();
            mymsg.header.frame_id="map";
            publisher_->publish(mymsg);

          std::size_t size;
          size = scan_p->angle_index.size();
          if(size!=0) 
          { 
            angle_cloud.clear();
            ag_cloud->clear();
           for (int i=0;i<size;i++)
            { // value = *it;
             angle_cloud.push_back(cloud_->points[scan_p->angle_index[i].index]);
             ag_cloud->add_point(cloud_->points[scan_p->angle_index[i].index],
                                  scan_p->angle_index[i].angle,
                                  scan_p->angle_index[i].distance,
                                  scan_p->angle_index[i].attribute
                                  );
               }
             ag_cloud->filename = "exabb.txt";
             ag_cloud->write_cloud();    
           pcl::toROSMsg(angle_cloud, angle_msg);
            angle_msg.header.stamp =now();
            angle_msg.header.frame_id="map";
            angle_pub_->publish(angle_msg);
            // ag_cloud->filename="exaa.txt";
            // ag_cloud->write_cloud();
          }
    RCLCPP_INFO(this->get_logger(),"have  a new  sum=%d angle=%d",cloud_->points.size(),scan_p->angle_index.size());
     if(save_flag==0)
     {
    try_find();save_flag=1;
     }
       }
//try data
  void try_find(void)
  {
      Angle_Point_Cloud *ag1;
      Angle_Point_Cloud *ag2;
      Convert_Seat *csp;
      ag1=new Angle_Point_Cloud();
      ag2=new Angle_Point_Cloud();
      csp = new Convert_Seat();
      ag1->filename="exabb1.txt";
      ag1->read_cloud();
      ag2->filename="exabb2.txt";
      ag2->read_cloud();
      ag1->claculate_distance();
      ag2->claculate_distance();
    std::fstream file("out.txt",std::ios::out);
    std::size_t size1,size2;
    if(file.is_open())
    {
        file<<"file1"<<std::endl;
        size1=ag1->my_point.size();
        for(int i=0;i<size1;i++)
        {
          size2=ag1->my_point[i].p_distance.size();
          file<<"point="<<i<<"  distance_no="<<size2<<std::endl;
         
          for(int j=0;j<size2;j++)
            file<<"j="<<j<<" "<<ag1->my_point[i].p_distance[j].first<<" "<<"distance="<<ag1->my_point[i].p_distance[j].second<<" ";
            file<<std::endl; 
        }
           file<<std::endl; file<<std::endl;
        size1=ag2->my_point.size();
        for(int i=0;i<size1;i++)
        {
          size2=ag2->my_point[i].p_distance.size();
          file<<"point="<<i<<"  distance_no="<<size2<<std::endl;
       
          for(int j=0;j<size2;j++)
            file<<"j="<<j<<" "<<ag2->my_point[i].p_distance[j].first<<" "<<"distance="<<ag2->my_point[i].p_distance[j].second<<" ";
            file<<std::endl; 
        }
        
        file.close();
    }
    csp->set_input_clouds(*ag1,*ag2);
    std::fstream file1("ppout.txt",std::ios::out);
    size1=csp->ppair.size();
    file1<<size1<<std::endl;
    for(int i=0;i<size1;i++)
      {
        file1<<csp->ppair[i].index0<<" "<<csp->ppair[i].index1<<" "
          <<csp->ppair[i].index2<<" "<<csp->ppair[i].index3<<std::endl;
      }
      file1.close();
  }
//try data end
    rclcpp::Subscription<sensor_msgs::msg::PointCloud2>::SharedPtr sub_bluesea_cloud;
    rclcpp::Subscription<sensor_msgs::msg::LaserScan>::SharedPtr sub_bluesea_scan;
    rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr publisher_;
    rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr angle_pub_;
    int count=0;float max_distance=0.0;
    int sum=0;
    float last_distance[2000];
    Scan_cloud *scan_p;
    Angle_Point_Cloud *ag_cloud;
    int save_flag=0;
};


int main(int argc, char * argv[])
{
  rclcpp::init(argc, argv);
  rclcpp::spin(std::make_shared<BlueseaSubScan>());
  
  rclcpp::shutdown();
  return 0;
}







